/*
        YXS隧道扫描数据处理 测试代码
*/
#include "../src/SGPCLDownSample.h"
#include "../src/SGPCLFeatureCalculator.h"
#include "../src/SGPCLReader.h"
#include "../src/SGPCLRegistration.h"
#include "../src/SGPCLVisual.h"
#include <pcl/filters/extract_indices.h>
#include <pcl/io/ply_io.h>

int main(int argc, char const* argv[])
{
#if defined(_WIN32) || defined(__WIN32__) || defined(WIN32) || defined(_WIN64) || defined(_WINDOWS)
  system("chcp 65001"); // 控制台设为UTF-8编码
#endif

  using PointT = pcl::PointXYZ;
  using PointCloud = pcl::PointCloud<PointT>;
  using PointCloudPtr = pcl::PointCloud<PointT>::Ptr;
  using PointCloudConstPtr = pcl::PointCloud<PointT>::ConstPtr;

  sgpcl::SGDownSampleConfig sDConfig;
  sDConfig.fVoxelSize = 0.05f;
  sDConfig.sVisConfig.bPlot = true;

  PointCloudPtr spDesign =
    sgpcl::ReadPCLFile<PointT>("F:\\20220530_PCL\\SGTunnelFace\\ZFSScanData\\data\\Design.ply");
  for (PointT& sPnt : spDesign->points)
    sPnt.z = 0;
  PointCloudPtr spDownSampledDesign = sgpcl::DownSample<PointT>(spDesign, sDConfig);
  pcl::io::savePLYFileASCII(
    "F:\\20220530_PCL\\SGTunnelFace\\ZFSScanData\\data\\SampledDesign.ply", *spDownSampledDesign);

  PointCloudPtr spScan =
    sgpcl::ReadPCLFile<PointT>("F:\\20220530_PCL\\SGTunnelFace\\ZFSScanData\\data\\Scan.ply");
  for (PointT& sPnt : spScan->points)
  {
    sPnt.x += .3;
    sPnt.y += .3;
    sPnt.z = 0;
  }
  PointCloudPtr spDownSampledScan = sgpcl::DownSample<PointT>(spScan, sDConfig);

  sgpcl::SGRegistrationConfig sConfig;
  sConfig.sVisConfig.bPlot = false;
  Eigen::Affine3f transform;
  pcl::IndicesPtr inliers(new pcl::Indices);
  sgpcl::CorrespondenceEstimation<PointT>(spDownSampledScan, spDownSampledDesign, sConfig, transform, inliers);
  // sgpcl::ICP<PointT>(spDownSampledScan, spDownSampledDesign, sConfig);

  pcl::transformPointCloud(*spDownSampledScan, *spDownSampledScan, transform); // 更新源点云
  

  // 创建过滤器对象
  pcl::ExtractIndices<PointT> extract;
  extract.setInputCloud(spDownSampledScan);
  extract.setIndices(inliers); //提取内点的索引并存储在其中
  extract.setNegative(false);

  PointCloudPtr spFinalScan(new PointCloud);
  extract.filter(*spFinalScan);

  // 可视化
  if (true)
  {
    // 画内外点云（内红色，外绿色）
    pcl::visualization::PCLVisualizer::Ptr spViewer =
      std::make_shared<pcl::visualization::PCLVisualizer>("correspond estimation");
    const float fPointSize = 2.f;

    // 设置点云颜色，添加点云数据
    const std::string strName1 = "Original";
    pcl::visualization::PointCloudColorHandlerCustom<PointT> rgb_1(spDownSampledScan, 255, 0, 0);
    spViewer->addPointCloud<PointT>(spDownSampledScan, rgb_1, strName1);
    // 设置点云的显示（渲染）大小
    spViewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, strName1);

    // 设置点云颜色，添加点云数据
    const std::string strName2 = "Design";
    pcl::visualization::PointCloudColorHandlerCustom<PointT> rgb_2(spDownSampledDesign, 0, 255, 0);
    spViewer->addPointCloud<PointT>(spDownSampledDesign, rgb_2, strName2);
    // 设置点云的显示（渲染）大小
    spViewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, strName2);

    // 设置点云颜色，添加点云数据
    const std::string strName3 = "Final";
    pcl::visualization::PointCloudColorHandlerCustom<PointT> rgb_3(spFinalScan, 0, 0, 255);
    spViewer->addPointCloud<PointT>(spFinalScan, rgb_3, strName3);
    // 设置点云的显示（渲染）大小
    spViewer->setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, strName3);

    while (!spViewer->wasStopped())
    {
      spViewer->spinOnce(100);
    }
  }

  return 0;
}
